#include "ArmorAssembler.h"

namespace hitcrt {
bool ArmorAssemblerBase::assemble(const std::vector<LightBar>& bars,
                                  const cv::Mat& srcImg,
                                  std::vector<Armor>& armors) {
    // 灯条太少肯定没有装甲板
    if (bars.size() < 2) {
        // std::cout << "灯条数量太少<2" << std::endl;
        return false;
    } else {
        m_srcImg = srcImg;  // 浅拷贝
        return findArmors(bars, armors);
    }
}
/**
 * @brief 寻找装甲板
 * @param[in] bars          My Param doc
 * @param[in] armors        My Param doc
 * @return true
 * @return false
 * @author LiuZhihao (2488288779@qq.com)
 */
bool ArmorAssemblerBase::findArmors(const std::vector<LightBar>& bars,
                                    std::vector<Armor>& armors) {
    cleanVectors();
    initClassifier();
    // 聚类，灯条两两匹配
    cv::partition(
        bars, m_labels,
        boost::bind(&hitcrt::ArmorAssemblerBase::isParallel, this, _1, _2));
    // std::cout << "聚类数量为： "
    //<< *max_element(m_labels.begin(), m_labels.end())+1<< std::endl;
    for (int i = *min_element(m_labels.begin(), m_labels.end());
         i <= *max_element(m_labels.begin(), m_labels.end()); i++) {
        // 构建排序依据
        auto idPairs = getParallelBarIndexs(bars, i);
        if (idPairs.size() > 1) {
            // 灯条排序
            sortParallelBars(idPairs);
            // 灯条组合成装甲板
            match(bars);
        }
    }

    return hasArmors(armors);
}
/**
 * @brief 清空vector成员变量
 * @author LiuZhihao (2488288779@qq.com)
 */
void ArmorAssemblerBase::cleanVectors() {
/// TODO 需要填写 ///
#pragma omp parallel for num_threads(12)
    m_labels.clear();
    m_labels.shrink_to_fit();
    m_searchFlags.clear();
    m_searchFlags.shrink_to_fit();
    m_indexs.clear();
    m_indexs.shrink_to_fit();
    m_barPairs.clear();
    m_barPairs.shrink_to_fit();
}
/**
 * @brief 判断平行灯条
 * @param[in] lightBar1     My Param doc
 * @param[in] lightBar2     My Param doc
 * @return true
 * @return false
 * @author LiuZhihao (2488288779@qq.com)
 */
bool ArmorAssembler::isParallel(LightBar lightBar1, LightBar lightBar2) {
    // bar之间夹角
    if (!checkProductImpl(lightBar1, lightBar2, m_parallelDeltaTheta)) {
        // std::cout << "等条夹角不符" << std::endl;
        return false;
    }
    // 灯条长度差
    if (!checkLengthDiffImpl(lightBar1, lightBar2, m_twoLengthScale)) {
        // std::cout << "等条长度差不符" << std::endl;
        return false;
    }
    // 装甲板长方形四个夹角
    if (!checkFourAngleImpl(lightBar1, lightBar2, m_cornerAngleMin,
                            m_cornerAngleMax)) {
        // std::cout << "等条装甲板长方形四个夹角差不符" << std::endl;
        return false;
    }
    return true;
}
/**
 * @brief 获得平行灯条的index并和其x坐标组成pair
 * @param[in] bars          My Param doc
 * @param[in] label         My Param doc
 * @return std::vector<std::pair<int, float>>
 * @author LiuZhihao (2488288779@qq.com)
 */
std::vector<std::pair<int, float>> ArmorAssembler::getParallelBarIndexs(
    const std::vector<LightBar>& bars, const int label) {
    std::vector<std::pair<int, float>> idPairs;
    for (int i = 0; i < bars.size(); i++) {
        if (m_labels[i] == label) {
            auto idpair = getIdPairImpl(i, bars);
            idPairs.emplace_back(idpair);
        }
    }
    return idPairs;
}
/**
 * @brief 排序规则
 * @param[in] indexPair1    My Param doc
 * @param[in] indexPair2    My Param doc
 * @return true
 * @return false
 * @author LiuZhihao (2488288779@qq.com)
 */
bool ArmorAssemblerBase::sortRuleImpl(const std::pair<int, float>& indexPair1,
                                      const std::pair<int, float>& indexPair2) {
    return indexPair1.second < indexPair2.second;  // 升序
}
/**
 * @brief 按照x轴坐标升序排序，存下排序前index用于后续下标引用bar
 * @param[in] indexPairs    My Param doc
 * @author LiuZhihao (2488288779@qq.com)
 */
void ArmorAssemblerBase::sortParallelBars(
    std::vector<std::pair<int, float>> indexPairs) {
    sortBarIndexImpl(indexPairs);
    m_indexs.clear();
    m_indexs.shrink_to_fit();
    for (int i = 0; i < indexPairs.size(); i++) {
        m_indexs.emplace_back(indexPairs[i].first);
    }
}
/**
 * @brief 使用sort进行排序
 * @param[in] indexPairs    My Param doc
 * @author LiuZhihao (2488288779@qq.com)
 */
void ArmorAssemblerBase::sortBarIndexImpl(
    std::vector<std::pair<int, float>>& indexPairs) {
    std::sort(
        indexPairs.begin(), indexPairs.end(),
        boost::bind(&hitcrt::ArmorAssemblerBase::sortRuleImpl, this, _1, _2));
}
/**
 * @brief 检查barPair是否为装甲板
 * @param[in] barPair       My Param doc
 * @return true
 * @return false
 * @author LiuZhihao (2488288779@qq.com)
 */
bool ArmorAssemblerBase::checkArmor(
    std::tuple<LightBar, LightBar, Pattern>& barPair) {
    auto lightBar1 = std::get<0>(barPair);
    auto lightBar2 = std::get<1>(barPair);
    if (!isParallel(lightBar1, lightBar2)) {
        // std::cout << "isparallel失败" << std::endl;
        return false;
    }

    // TODO
    // 宽高比检查m_distanceLengthScaleMin与m_distanceLengthScaleMax，此处也需要你来填写
    double distanceLengthScale =
        std::sqrt(std::pow(lightBar1.m_top.x - lightBar2.m_top.x, 2) +
                  std::pow(lightBar1.m_top.y - lightBar2.m_top.y, 2)) /
        std::sqrt(std::pow(lightBar1.m_top.x - lightBar1.m_bottom.x, 2) +
                  std::pow(lightBar1.m_top.y - lightBar1.m_bottom.y, 2));
    if (distanceLengthScale > m_distanceLengthScaleMax ||
        distanceLengthScale < m_distanceLengthScaleMin) {
        // std::cout << "宽高比不符合" << std::endl;
        // std::cout << "distanceLengthScale" << distanceLengthScale <<
        // std::endl; std::cout << "m_distanceLengthScaleMin" <<
        // m_distanceLengthScaleMin
        //<< std::endl;
        // std::cout << "m_distanceLengthScaleMax" << m_distanceLengthScaleMax
        // << std::endl;
        return false;
    }
    // TODO 灯条中心连线与水平方向夹角检查m_xDistanceScale，此处也需要你来填写
    // x轴 y的方向向量为零
    double xDistanceScale =
        acos((lightBar1.m_center.x - lightBar2.m_center.x) * 1.0 /
                 std::pow(lightBar1.m_center.x - lightBar2.m_center.x, 2) +
             std::pow(lightBar1.m_center.y - lightBar2.m_center.y, 2));
    if (xDistanceScale > m_xDistanceScale) {
        // std::cout << "水平倾斜" << std::endl;
        // std::cout << "xDistanceScale" << xDistanceScale << std::endl;
        return false;
    }
    // 分类大小装甲板
    if (distanceLengthScale > m_sizedistanceLengthScale) {
    }
    // 上述如果都通过说明装甲板匹配成功
    // 在此加入装甲板大小识别的的分类

    std::vector<cv::Point2f> srcPoints;
    srcPoints.reserve(4);

    srcPoints.push_back(std::get<0>(barPair).m_bottom /*bottom left*/);
    srcPoints.push_back(std::get<1>(barPair).m_bottom /*bottom right*/);
    srcPoints.push_back(std::get<0>(barPair).m_top /*top left*/);
    srcPoints.push_back(std::get<1>(barPair).m_top /*top right*/);

    // TimePoint startTime, endTime;
    // startTime = std::chrono::steady_clock::now();
    Pattern pattern = m_classifier->apply(srcPoints, m_srcImg);
    // std::cout << "pattern = " << pattern
    //         << std::endl;  // 这里测试数字识别是否通过
    // endTime = std::chrono::steady_clock::now();
    // auto duration = endTime - startTime;
    // std::cout << (double)duration.count() / std::nano::den * 1000 <<
    // std::endl;  // 单位ms

    if (pattern == Pattern::UNKNOWN) {
        return false;
    } else {
        std::get<2>(barPair) = pattern;
        //std::cout << "pattren: " << pattern << std::endl;
        return true;
    }

    return true;
}
/**
 * @brief 创建一个barpair
 * @param[in] bar1          My Param doc
 * @param[in] bar2          My Param doc
 * @return std::tuple<LightBar, LightBar, Pattern>
 * @author LiuZhihao (2488288779@qq.com)
 */
std::tuple<LightBar, LightBar, Pattern> ArmorAssembler::makeBarPairImpl(
    const LightBar& bar1, const LightBar& bar2) {
    /// TODO 需要填写 ///
    Pattern pattern;
    std::tuple<LightBar, LightBar, Pattern> barPair(bar1, bar2, pattern);
    return barPair;
}
/**
 * @brief 平行灯条两两匹配
 * @param[in] bars          My Param doc
 * @author LiuZhihao (2488288779@qq.com)
 */
void ArmorAssembler::match(const std::vector<LightBar>& bars) {
    std::vector<bool>(m_indexs.size(), false).swap(m_searchFlags);
    /// TODO match策略需要填写 需要用到makeBarPairImpl与checkArmor///
    std::tuple<LightBar, LightBar, Pattern> barPair;
    for (int i = 0; i < m_indexs.size(); i++) {
        for (int j = i + 1; j < m_indexs.size(); j++) {
            if (!(m_searchFlags[m_indexs[i]] || m_searchFlags[m_indexs[j]])) {
                barPair = makeBarPairImpl(bars[m_indexs[i]], bars[m_indexs[j]]);
                if (checkArmor(barPair)) {
                    m_barPairs.emplace_back(barPair);
                    m_searchFlags[m_indexs[i]] = true;
                    m_searchFlags[m_indexs[j]] = true;
                }
            }
        }
    }
}

/**
 * @brief 创建一个装甲板
 * @param[in] barPair       My Param doc
 * @return Armor
 * @author LiuZhihao (2488288779@qq.com)
 */
Armor ArmorAssembler::createOneArmorImpl(
    std::tuple<LightBar, LightBar, Pattern> barPair) {
    /// TODO 需要填写 ///
    auto lightBar1 = std::get<0>(barPair);
    auto lightBar2 = std::get<1>(barPair);
    auto pattern = std::get<2>(barPair);
    // 这里没看到装甲板<Armor>的构造函数用到<Pattern>
    return Armor(lightBar1, lightBar2, pattern);
}
/**
 * @brief 创建装甲板
 * @param[in] armors        My Param doc
 * @author LiuZhihao (2488288779@qq.com)
 */
void ArmorAssembler::createArmors(std::vector<Armor>& armors) {
    for (int i = 0; i < m_barPairs.size(); i++) {
        armors.emplace_back(createOneArmorImpl(m_barPairs[i]));
    }
}
/**
 * @brief 判断是否存在装甲板并创建装甲板
 * @param[in] armors        My Param doc
 * @return true
 * @return false
 * @author LiuZhihao (2488288779@qq.com)
 */
bool ArmorAssemblerBase::hasArmors(std::vector<Armor>& armors) {
    createArmors(armors);
    // 在识别出装甲板后 区分大小装甲板
    sizeClassification(armors);
    return armors.size();
}

bool ArmorAssembler::checkFourAngleImpl(const LightBar& lightBar1,
                                        const LightBar& lightBar2,
                                        const float cornerAngleMin,
                                        const float cornerAngleMax) {
    /// TODO 需要填写 ///
    // 计算灯条所构成四边形四个角的四组向量
    // #pragma omp parallel for num_threads(12)
    std::vector<cv::Point2f> AngleVector;
    AngleVector.push_back(
        cv::Point2f(lightBar1.m_top.x - lightBar1.m_bottom.x,
                    lightBar1.m_top.y - lightBar1.m_bottom.y));
    AngleVector.push_back(cv::Point2f(lightBar1.m_top.x - lightBar2.m_top.x,
                                      lightBar1.m_top.y - lightBar2.m_top.y));
    AngleVector.push_back(
        cv::Point2f(lightBar2.m_top.x - lightBar2.m_bottom.x,
                    lightBar2.m_top.y - lightBar2.m_bottom.y));
    AngleVector.push_back(
        cv::Point2f(lightBar2.m_bottom.x - lightBar1.m_bottom.x,
                    lightBar2.m_bottom.y - lightBar1.m_bottom.y));
    double cornerAngle[4];
    double lengthVector[4];

    lengthVector[0] = std::sqrt(std::pow(AngleVector[0].x, 2) +
                                std::pow(AngleVector[0].y, 2));
    lengthVector[1] = std::sqrt(std::pow(AngleVector[1].x, 2) +
                                std::pow(AngleVector[1].y, 2));
    lengthVector[2] = std::sqrt(std::pow(AngleVector[2].x, 2) +
                                std::pow(AngleVector[2].y, 2));
    lengthVector[3] = std::sqrt(std::pow(AngleVector[3].x, 2) +
                                std::pow(AngleVector[3].y, 2));

    cornerAngle[0] = acos((AngleVector[0].x * AngleVector[1].x +
                           AngleVector[0].y * AngleVector[1].y) /
                          lengthVector[0] / lengthVector[1]);
    cornerAngle[1] = acos((AngleVector[1].x * AngleVector[2].x +
                           AngleVector[1].y * AngleVector[2].y) /
                          lengthVector[1] / lengthVector[2]);
    cornerAngle[2] = acos((AngleVector[2].x * AngleVector[3].x +
                           AngleVector[2].y * AngleVector[3].y) /
                          lengthVector[2] / lengthVector[3]);
    cornerAngle[3] = acos((AngleVector[3].x * AngleVector[0].x +
                           AngleVector[3].y * AngleVector[0].y) /
                          lengthVector[3] / lengthVector[0]);
    // 有点啰嗦
    if (!(cornerAngle[0] < cornerAngleMax && cornerAngle[0] > cornerAngleMin &&
          cornerAngle[1] < cornerAngleMax && cornerAngle[1] > cornerAngleMin &&
          cornerAngle[2] < cornerAngleMax && cornerAngle[2] > cornerAngleMin &&
          cornerAngle[3] < cornerAngleMax && cornerAngle[3] > cornerAngleMin)) {
        // std::cout << "cornerAngle[0]:" << cornerAngle[0] << " "
        //  << "cornerAngle[1]:" << cornerAngle[1] << " "
        //  << "cornerAngle[2]:" << cornerAngle[2] << " "
        //  << "cornerAngle[3]:" << cornerAngle[3] << std::endl;
        // std::cout << "cornerAngleMin:" << cornerAngleMin << " "
        //<< "cornerAngleMax:" << cornerAngleMax << std::endl;
    }

    return cornerAngle[0] < cornerAngleMax && cornerAngle[0] > cornerAngleMin &&
           cornerAngle[1] < cornerAngleMax && cornerAngle[1] > cornerAngleMin &&
           cornerAngle[2] < cornerAngleMax && cornerAngle[2] > cornerAngleMin &&
           cornerAngle[3] < cornerAngleMax && cornerAngle[3] > cornerAngleMin;
}
/**
 * @brief 叉乘得到灯条夹角
 * @param[in] lightBar1     My Param doc
 * @param[in] lightBar2     My Param doc
 * @param[in] deltaThetaThresh My Param doc
 * @return true
 * @return false
 * @author LiuZhihao (2488288779@qq.com)
 */
bool ArmorAssembler::checkProductImpl(const LightBar& lightBar1,
                                      const LightBar& lightBar2,
                                      const float deltaThetaThresh) {
    /// TODO 需要填写 ///
    float deltaTheta;
    cv::Point2f VectorBar1(lightBar1.m_top.x - lightBar1.m_bottom.x,
                           lightBar1.m_top.y - lightBar1.m_bottom.y);
    cv::Point2f VectorBar2(lightBar2.m_top.x - lightBar2.m_bottom.x,
                           lightBar2.m_top.y - lightBar2.m_bottom.y);
    deltaTheta = std::abs(asin(
        ((VectorBar1.x * VectorBar2.y - VectorBar1.y * VectorBar2.x) /
         std::sqrt(VectorBar1.x * VectorBar1.x + VectorBar1.y * VectorBar1.y)) /
        std::sqrt(VectorBar2.x * VectorBar2.x + VectorBar2.y * VectorBar2.y)));
    if (!(deltaTheta < deltaThetaThresh)) {
        // std::cout << "deltaTheta: " << deltaTheta << " " <<
        // "deltaThetaThresh"
        //<< deltaThetaThresh << std::endl;
    }
    return deltaTheta < deltaThetaThresh;
}
/**
 * @brief 检查长度比
 * @param[in] lightBar1     My Param doc
 * @param[in] lightBar2     My Param doc
 * @param[in] twoLengthScale My Param doc
 * @return true
 * @return false
 * @author LiuZhihao (2488288779@qq.com)
 */
bool ArmorAssembler::checkLengthDiffImpl(const LightBar& lightBar1,
                                         const LightBar& lightBar2,
                                         const float twoLengthScale) {
    /// TODO 需要填写 ///
    double LengthRatio = lightBar1.m_length / lightBar2.m_length;
    if (!(LengthRatio < twoLengthScale && (1 / LengthRatio) < twoLengthScale)) {
        // std::cout << "LengthRatio: " << LengthRatio << " "
        //  << "twoLengthScale: " << twoLengthScale << std::endl;
    }

    return (LengthRatio < twoLengthScale && (1 / LengthRatio) < twoLengthScale);
}
/**
 * @brief 创建idpair
 * @param[in] id            My Param doc
 * @param[in] bars          My Param doc
 * @return std::pair<int, float>
 * @author LiuZhihao (2488288779@qq.com)
 */
std::pair<int, float> ArmorAssembler::getIdPairImpl(
    const int id, const std::vector<LightBar>& bars) {
    /// TODO 需要填写 ///
    return std::pair<int, float>(id, bars[id].m_center.x);
}

/**
 * @brief 区分大小装甲板
 * @param[in] armors         MyParam doc
 * @return * void
 * @author fx_03 (18249293810@qq.com)
 */

void ArmorAssembler::sizeClassification(std::vector<Armor>& armors) {
    for (int i = 0; i < armors.size(); i++) {
        // armors[i].m_size = (armors[i].m_aspectRatio <
        // m_sizedistanceLengthScale)
        //                        ? SMALL
        //                        : LARGE;
        armors[i].m_size = LARGE;
    }
}

void ArmorAssembler::initClassifier() {}

}  // namespace hitcrt
